#ifndef BTL_CAMERA_CAMERAMODEL
#define BTL_CAMERA_CAMERAMODEL

#include <Eigen/Dense>

namespace btl {
namespace camera {

class CameraModel
{

   public:

      virtual ~CameraModel();

};

class CameraModelPinhole : virtual public CameraModel
{

   public:

      CameraModelPinhole(
         int imageResolutionX, int imageResolutionY,
         double focalLength);

      CameraModelPinhole(
         const Eigen::Vector2i& imageResolution,
         double focalLength);

      CameraModelPinhole(
         int imageResolutionX, int imageResolutionY,
         double focalLength,
         double principlePointX, double principlePointY);

      CameraModelPinhole(
         const Eigen::Vector2i& imageResolution,
         double focalLength,
         const Eigen::Vector2d& principlePoint);

      CameraModelPinhole(
         int imageResolutionX, int imageResolutionY,
         double focalLengthX, double focalLengthY);

      CameraModelPinhole(
         const Eigen::Vector2i& imageResolution,
         const Eigen::Vector2d& focalLength);

      CameraModelPinhole(
         int imageResolutionX, int imageResolutionY,
         double focalLengthX, double focalLengthY,
         double principlePointX, double principlePointY);

      CameraModelPinhole(
         const Eigen::Vector2i& imageResolution,
         const Eigen::Vector2d& focalLength,
         const Eigen::Vector2d& principlePoint);

      virtual ~CameraModelPinhole();

      virtual Eigen::Vector2d projectPoint(const Eigen::Vector3d& unprojected);

      virtual Eigen::Vector3d unprojectPoint(const Eigen::Vector2d& projected);

   protected:

      void createProjectionMatrix();

      Eigen::Vector2i _imageResolution;

      Eigen::Vector2d _focalLength;

      Eigen::Vector2d _principlePoint;

      Eigen::Matrix3d _projectionMatrix;

      Eigen::Matrix3d _invProjectionMatrix;

};

class CameraModelDistorted : virtual public CameraModel
{

   public:

      virtual ~CameraModelDistorted();

      virtual Eigen::Vector2d distortPoint(const Eigen::Vector2d& undistorted) = 0;

};

class CameraModelDistortedBidirectional : public CameraModelDistorted
{

   public:

      virtual ~CameraModelDistortedBidirectional();

      virtual Eigen::Vector2d undistortPoint(const Eigen::Vector2d& distorted) = 0;

};

} //namespace camera
} //namespace btl

namespace btl {

   using camera::CameraModel;
   using camera::CameraModelPinhole;
   using camera::CameraModelDistorted;
   using camera::CameraModelDistortedBidirectional;

} //namespace btl

namespace btl {
namespace camera {

inline CameraModel::~CameraModel() {}

inline CameraModelPinhole::CameraModelPinhole(
   int imageResolutionX, int imageResolutionY,
   double focalLength
) :
   _imageResolution(imageResolutionX, imageResolutionY),
   _focalLength(focalLength, focalLength),
   _principlePoint(_imageResolution.cast<double>() * 0.5)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   const Eigen::Vector2i& imageResolution,
   double focalLength
) :
   _imageResolution(imageResolution),
   _focalLength(focalLength, focalLength),
   _principlePoint(_imageResolution.cast<double>() * 0.5)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   int imageResolutionX, int imageResolutionY,
   double focalLength,
   double principlePointX, double principlePointY
) :
   _imageResolution(imageResolutionX, imageResolutionY),
   _focalLength(focalLength, focalLength),
   _principlePoint(principlePointX, principlePointY)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   const Eigen::Vector2i& imageResolution,
   double focalLength,
   const Eigen::Vector2d& principlePoint
) :
   _imageResolution(imageResolution),
   _focalLength(focalLength, focalLength),
   _principlePoint(principlePoint)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   int imageResolutionX, int imageResolutionY,
   double focalLengthX, double focalLengthY
) :
   _imageResolution(imageResolutionX, imageResolutionY),
   _focalLength(focalLengthX, focalLengthY),
   _principlePoint(_imageResolution.cast<double>() * 0.5)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   const Eigen::Vector2i& imageResolution,
   const Eigen::Vector2d& focalLength
) :
   _imageResolution(imageResolution),
   _focalLength(focalLength),
   _principlePoint(_imageResolution.cast<double>() * 0.5)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   int imageResolutionX, int imageResolutionY,
   double focalLengthX, double focalLengthY,
   double principlePointX, double principlePointY
) :
   _imageResolution(imageResolutionX, imageResolutionY),
   _focalLength(focalLengthX, focalLengthY),
   _principlePoint(principlePointX, principlePointY)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::CameraModelPinhole(
   const Eigen::Vector2i& imageResolution,
   const Eigen::Vector2d& focalLength,
   const Eigen::Vector2d& principlePoint
) :
   _imageResolution(imageResolution),
   _focalLength(focalLength),
   _principlePoint(principlePoint)
{
   createProjectionMatrix();
}

inline CameraModelPinhole::~CameraModelPinhole() {}

inline Eigen::Vector2d CameraModelPinhole::projectPoint(const Eigen::Vector3d& unprojected)
{
   Eigen::Vector3d projected3(_projectionMatrix * unprojected);
   double invZ = 1.0 / projected3(2);
   Eigen::Vector2d projected(projected3(0) * invZ, projected3(1) * invZ);
   return projected;
}

inline Eigen::Vector3d CameraModelPinhole::unprojectPoint(const Eigen::Vector2d& projected)
{
   Eigen::Vector3d projected3(projected(0), projected(1), 1.0);
   Eigen::Vector3d unprojected(_invProjectionMatrix * projected3);
   return unprojected;
}

inline void CameraModelPinhole::createProjectionMatrix()
{
   _projectionMatrix << _focalLength(0),               0, _principlePoint(0),
                                      0, _focalLength(1), _principlePoint(1),
                                      0,               0,                  1;
   _invProjectionMatrix = _projectionMatrix.inverse();
}

inline CameraModelDistorted::~CameraModelDistorted() {}

inline CameraModelDistortedBidirectional::~CameraModelDistortedBidirectional() {}


} //namespace camera
} //namespace btl

#endif //BTL_CAMERA_CAMERAMODEL
